#ifndef READ_DATA_H
#define READ_DATA_H
#include <iostream>
#include <sstream>
#include <fstream>
#include <vector>
#include <unistd.h>
#include <dirent.h>
#include <stdlib.h>
#include <stdio.h>
#include <map>
#include <queue>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <unordered_map>
#include <algorithm>
#include <memory>
#include <unordered_set>
#include <tuple>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include "node/node.h"

using namespace std;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

struct Options {
    double odom_var_ = 0.05;
    double odom_span_ = 0.01;        // 里程计测量间隔
    double left_wheel_radius_ = 0.623479 / 2.0;   // 轮子半径
    double right_wheel_radius_ = 0.622806 / 2.0;   // 轮子半径
    double circle_pulse_ = 4096.0;  // 编码器每圈脉冲数
    double wheel_base_ = 1.52439;
    //double kl_ = 0.00047820240382508;
    //double kr_ = 0.00047768621928995;

    double kl_ = 1.0 / circle_pulse_ * M_PI * (2.0 * left_wheel_radius_);
    double kr_ = 1.0 / circle_pulse_ * M_PI * (2.0 * right_wheel_radius_);
};

struct LaserScan2d {
    uint64_t timestamp;
    std::vector<float> distances;
    std::vector<float> intensities;  // 新增强度值
    std::vector<float> x_coords;
    std::vector<float> y_coords;
};

template<typename T>
Eigen::Matrix<T,3,3> eulerToRotationMatrix(T roll, T pitch, T yaw) {
    // 转换为弧度
    roll *= M_PI/180.0f;
    pitch *= M_PI/180.0f;
    yaw *= M_PI/180.0f;
    
    // 计算三角函数值
    T cr = cos(roll); T sr = sin(roll);
    T cp = cos(pitch); T sp = sin(pitch);
    T cy = cos(yaw); T sy = sin(yaw);
    
    // 构建旋转矩阵 (Z-Y-X顺序)
    Eigen::Matrix<T,3,3> R;
    R << cy*cp,  cy*sp*sr - sy*cr,  cy*sp*cr + sy*sr,
         sy*cp,  sy*sp*sr + cy*cr,  sy*sp*cr - cy*sr,
         -sp,    cp*sr,            cp*cr;
    
    return R;
}
inline std::string& ltrim(std::string &s) {
    s.erase(s.begin(), std::find_if(s.begin(), s.end(), 
        [](int ch) { return !std::isspace(ch); }));
    return s;
}

// 去除右侧空白字符
inline std::string& rtrim(std::string &s) {
    s.erase(std::find_if(s.rbegin(), s.rend(), 
        [](int ch) { return !std::isspace(ch); }).base(), s.end());
    return s;
}

// 去除两侧空白字符
inline std::string& trim(std::string &s) {
    return ltrim(rtrim(s));
}

// 返回新字符串的版本（不修改原字符串）
inline std::string trim_copy(std::string s) {
    trim(s);
    return s;
}

enum class SensorDataType {
    KAIST = 0,
    NCLT
};

class read_data {
public:
    read_data(const std::string& path, const SensorDataType& sensor_data_type = SensorDataType::KAIST);
    
    bool LoadSensorData(const std::string& encoder_file_path, std::unordered_map<std::string, std::string>* time_data_map);

    bool read_only_kaist_data(std::unordered_map<std::string, std::string>& time_encoder_map, 
                              std::unordered_map<std::string, std::string>& time_imu_map,
                              std::unordered_map<std::string, std::string>& time_gps_map, 
                              std::unordered_map<std::string, std::string>& time_vrs_gps_map,
                              std::unordered_map<std::string, std::string>& time_sick_middle_map,
                              std::unordered_map<std::string, std::string>& time_velodyne_left_map);

    std::unordered_map<std::string, std::string> read_only_nclt_data(const std::string& file_path);
    
    std::unordered_map<std::string, LaserScan> read_only_nclt_laser_data(const std::string& file_path);

    double get_origin_timestamp() {
        return origin_timestamp;
    }

    Eigen::Matrix3f get_ext_rot() {
        return ext_rot_vehicle2laser_;
    }
    Eigen::Vector3f get_ext_trans() {
        return ext_trans_vehicle2laser_;
    }

    Eigen::Matrix4f get_ext() {
        Eigen::Matrix4f Tvl = Eigen::Matrix4f::Identity();
        Tvl.block<3,3>(0,0) = ext_rot_vehicle2laser_;
        Tvl.block<3,1>(0,3) = ext_trans_vehicle2laser_;
        return Tvl;
    }
private:
    std::string path_;

    std::vector<std::string> split(std::string str, std::string pattern);

    double origin_timestamp = 0;

    Eigen::Matrix3f ext_rot_vehicle2laser_;
    Eigen::Vector3f ext_trans_vehicle2laser_;

    SensorDataType sensor_data_type_;
};

#endif


